//
// Created by Li Yi liang.
//
#include "mavlink/v2.0/standard/mavlink.h"
#include "geo/geo.h"
#include <iostream>
#include <unistd.h>
#include <thread>
#include <vector>
#include <cmath>
#include <sys/socket.h>
#include <arpa/inet.h>



int recv_px4_sock_fd_ = -1;
struct sockaddr_in st_send_px4_addr_{};

void sub_udp(mavlink_global_position_int_t &pos_){
    // 创建UDP socket
    recv_px4_sock_fd_ = socket(AF_INET, SOCK_DGRAM, 0);
    // 绑定本地地址和端口
    struct sockaddr_in st_recv_px4_addr_{};
    st_recv_px4_addr_.sin_family = AF_INET;
    st_recv_px4_addr_.sin_addr.s_addr = htonl(INADDR_ANY);
    st_recv_px4_addr_.sin_port = htons(26540);

    if (bind(recv_px4_sock_fd_, (struct sockaddr*)&st_recv_px4_addr_, sizeof(struct sockaddr)) == -1)
    {
        std::cerr << "bind failed!!!" << std::endl;
    }

    // 循环收消息
    char buffer[1024];
    socklen_t addr_len = sizeof(struct sockaddr);
    struct sockaddr_in px4_addr{};
    while(true) {
        mavlink_message_t msg;
        mavlink_status_t mavlinkStatus;

        ssize_t len = recvfrom(recv_px4_sock_fd_, buffer, sizeof(buffer), 0, (struct sockaddr *) &px4_addr, &addr_len);
        if (len < 0) continue;

        for (size_t i = 0; i < len; i++){
            if (mavlink_parse_char(MAVLINK_COMM_0, buffer[i], &msg, &mavlinkStatus) && msg.sysid != 255){  // 255是地面站的默认ID
                if(int(msg.msgid) == MAVLINK_MSG_ID_GLOBAL_POSITION_INT){
                    mavlink_global_position_int_t global_pos_;
                    mavlink_msg_global_position_int_decode(&msg, &global_pos_);
                    pos_ = global_pos_;
//                    std::cout << "global pos : lat = " << pos_->lat << ", lon = " << pos_->lon << ", alt = " << pos_->alt << std::endl;
                    break;
                }
            }
        }
        usleep(2*1000); // 2ms
    }
}

void get_waypoint(mavlink_global_position_int_t &pos_, mavlink_set_position_target_local_ned_t &point_){
    std::vector<float> point1_ = {500,0,-50};
    std::vector<float> point2_ = {500,500,-50};
    int point_flag = 1;
    float my_x = 0, my_y = 0, my_z = 0;
    while(true){
        std::cout << "1 " << my_x << " , " << my_y << std::endl;
        // 将经纬转化成xy(m)并存入变量
        globallocalconverter_tolocal(double(pos_.lat * 1e-7), double(pos_.lon * 1e-7),float(pos_.alt * 1e-3), &my_x, &my_y, &my_z);

        // 判断是否切换到第二个航点
        if(sqrt(pow((point1_[0]-my_x),2) + pow((point1_[1]-my_y),2)) < 30) point_flag = 2;
        // 将航点存入用于发布的结构体
        point_.coordinate_frame = MAV_FRAME_LOCAL_NED;
        point_.target_system = 1;
        point_.target_component = 1;
        if (point_flag == 1) {
            point_.type_mask = 0b0001111111111000; // 起飞点
            point_.x = point1_[0];
            point_.y = point1_[1];
            point_.z = -50;
        } else {
            point_.type_mask = 0b0000111111111000; // 普通航点
            point_.x = point1_[0];
            point_.y = point2_[1];
            point_.z = -50;
        }
        usleep(100*1000);
    }
}


void pub_udp(mavlink_set_position_target_local_ned_t &point_){
    mavlink_message_t msg;
    ssize_t buflen;
    uint8_t sendbuf[2048];
    st_send_px4_addr_.sin_family = AF_INET;
    st_send_px4_addr_.sin_addr.s_addr = htonl(INADDR_ANY);
    st_send_px4_addr_.sin_port = htons(24580);
    while(true){
        std::cout << "2 " << point_.x << " , " << point_.y << std::endl;
        mavlink_msg_set_position_target_local_ned_encode(255, 190, &msg, &point_);
        buflen = mavlink_msg_to_send_buffer(sendbuf, &msg);
        ssize_t send_len = sendto(recv_px4_sock_fd_, sendbuf, buflen, 0, (struct sockaddr *) &st_send_px4_addr_, sizeof(struct sockaddr_in));
        if (send_len <= 0) {
            perror("Failed to send msg to px4 by UDP");
        }
        usleep(100 * 1000);
    }
}

void arm_and_offboard(){
    // arm
    mavlink_command_int_t cmd;
    mavlink_message_t msg;
    uint8_t sendbuf[128];
    memset(&cmd, 0, sizeof(cmd));
    cmd.target_system = 1; // 和QGC的飞控id对应
    cmd.target_component = MAV_COMP_ID_ALL;
    cmd.command = MAV_CMD::MAV_CMD_COMPONENT_ARM_DISARM;
    cmd.param1 = 1;
    cmd.param2 = 21196;
    mavlink_msg_command_int_encode(255, 190, &msg, &cmd);
    ssize_t buflen1_ = mavlink_msg_to_send_buffer(sendbuf, &msg);
    ssize_t send_len_1 = sendto(recv_px4_sock_fd_, sendbuf, buflen1_, 0, (struct sockaddr *) &st_send_px4_addr_, sizeof(struct sockaddr_in));
    if (send_len_1 <= 0) {
        perror("Failed to send arm to px4 by UDP");
    }

    // offboard
    memset(&cmd, 0, sizeof(cmd));
    cmd.target_system = 1;
    cmd.target_component = MAV_COMP_ID_ALL;
    cmd.command = MAV_CMD::MAV_CMD_DO_SET_MODE;
    cmd.param1 = 1;
    cmd.param2 = 6; // 6 -> offboard
    mavlink_msg_command_int_encode(255, 190, &msg, &cmd);
    ssize_t buflen2_ = mavlink_msg_to_send_buffer(sendbuf, &msg);
    ssize_t send_len_2 = sendto(recv_px4_sock_fd_, sendbuf, buflen2_, 0, (struct sockaddr *) &st_send_px4_addr_, sizeof(struct sockaddr_in));
    if (send_len_2 <= 0) {
        perror("Failed to send offboard to px4 by UDP");
    }
}

int main() {
    mavlink_global_position_int_t globalPos;           // 用于存储消息
    mavlink_set_position_target_local_ned_t waypoint;  // 用于发布位置

    // 接收消息的线程
    std::thread get_info(sub_udp, std::ref(globalPos));

    // 起始点作为xy原点，经纬系转北东地
    usleep(1*1000*1000); // 等待1s确定接收到消息后再开始
    globallocalconverter_init(double(globalPos.lat * 1e-7), double(globalPos.lon * 1e-7), float(globalPos.alt * 1e-3), globalPos.time_boot_ms);

    // 计算航点的线程
    std::thread get_way(get_waypoint,std::ref(globalPos), std::ref(waypoint));

    // 发布消息的线程
    while(waypoint.x == 0 && waypoint.y == 0){
        usleep(0.5*1000*1000);
    }
    std::thread pub_info(pub_udp, std::ref(waypoint));

    // 飞机解锁&切换offboard模式
    usleep(1*1000*1000);
    arm_and_offboard();

    get_info.join();
    pub_info.join();
    get_way.join();
    return 0;
}

